// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PicknPlaceGenericArmFingerGripper.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware
{
    using System;
    using Microsoft.Robotics.Manipulation.PicknPlace;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Pick and place grips for a generic robot arm and a generic gripper with fingers.
    /// Contains generic interface implementations that real arm / gripper combinations can use
    /// </summary>
    public abstract class PicknPlaceGenericArmFingerGripper : PicknPlaceHardwareGrip
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="PicknPlaceGenericArmFingerGripper" /> class
        /// </summary>
        public PicknPlaceGenericArmFingerGripper()
        {
            double cosAng = Vector3.Dot(this.PalmPlaneDefault, this.PalmPlaneGripAxis);
            if (cosAng > MathConstants.ErrorEpsilon)
            {
                throw new Exception("Hand grip axis must be orthogonal to palm normal");
            }
        }

        /// <summary>
        /// Gets or sets offset of the arm tool control point from the gripper palm plane in meters along the palm normal
        /// </summary>
        protected double ArmTCPPalmOffsetInM { get; set; }

        /// <summary>
        /// Gets or sets  default hand palm normal in arm coordinate space
        /// </summary>
        protected Vector3 PalmPlaneDefault { get; set; }

        /// <summary>
        /// Gets or sets default grip axis for hand in palm plane in arm space. Experimentally measured from equipment in mm
        /// </summary>
        protected Vector3 PalmPlaneGripAxis { get; set; }

        /// <summary>
        /// Gets or sets simple model of model of the Robotiq hand as a box attached to palm. Used to estimate minimum clearance from a 
        /// surface parallel to XY plane
        /// </summary>
        protected Vector3 SimpleGripperBoxModel { get; set; }

        /// <summary>
        /// Gets or sets pre grip offset along the grip normal
        /// </summary>
        protected double PreGripOffsetAlongGripNormalInM { get; set; }

        /// <summary>
        /// Gets or sets closest location to arm base that can be reached end is in top down position (Experimentally determined) in meter
        /// </summary>
        protected double MinArmReachInMForTopDown { get; set; }

        /// <summary>
        /// Gets or sets closest location to arm base that can be reached (Experimentally determined) in meter
        /// </summary>
        protected double MinArmReachInM { get; set; }

        /// <summary>
        /// Gets or sets furthest location from arm base that can be reached with arm fully extended (Experimentally determined) in meter
        /// </summary>
        protected double MaxArmReachInMForTopDown { get; set; }

        /// <summary>
        /// Gets or sets furthest location from arm base that can be reached with arm fully extended (Experimentally determined) in meter
        /// </summary>
        protected double MaxArmReachInM { get; set; }

        /// <summary>
        /// Gets or sets furthest location from arm base that can be reached with arm fully extended in XY plane (Experimentally determined) in meter
        /// </summary>
        protected double MaxArmReachXYInM { get; set; }

        /// <summary>
        /// Gets or sets furthest location from arm base that can be reached with arm fully extended in XY plane (Experimentally determined) in meter
        /// </summary>
        protected double MaxArmReachXYInMForTopDown { get; set; }

        /// <summary>
        /// Gets or sets distance threshold for fully extended Top down grasp arm
        /// </summary>
        protected double MaxFullyExtendXYForTopDown { get; set; }

        /// <summary>
        /// Gets or sets maximum elevation when fully extended in top down
        /// </summary>
        protected double MaxFullyExtendElevationForTopDown { get; set; }

        /// <summary>
        /// Gets or sets temporary safety clearance for end effector to avoid colliding with table
        /// </summary>
        protected double MaxYclearance { get; set; }

        /// <summary>
        /// Gets or sets temporary safety clearance for table avoidance
        /// </summary>
        protected double MinZClearanceOverTable { get; set; }

        /// <summary>
        /// Gets or sets temporary check for cases when gripper and arm point in opposite directions as cosine of angle between 
        /// control point vector and gripper palm surface
        /// </summary>
        protected double MinArmGripperOrientThreshold { get; set; }
        
        /// <summary>
        /// Gets or sets maximum height for Wrap around fully retracted position
        /// </summary>
        protected double MaxFullyRetractedElevationWrap { get; set; }

        /// <summary>
        /// Gets or sets minimum distance for wrap around
        /// </summary>
        protected double MinReachXYForWrap { get; set; }

        /// <summary>
        /// Gets or sets the rate of gripper closure rate in percent per meter. This simply the slope
        /// of gripper aperture size in percentage against gripper opening in meter
        /// </summary>
        protected double GripperRatePerM { get; set; }

        /// <summary>
        /// Gets or sets gripper percentage opening when the grip aperture  is zero
        /// </summary>
        protected double GripperOpenPercentOffset { get; set; }

        /// <summary>
        /// Gets or sets name of arm
        /// </summary>
        protected string ArmName { get; set; }

        /// <summary>
        /// Return the tool control point pose for a grip
        /// </summary>
        /// <param name="grip">The grip</param>
        /// <param name="surfaceOffset">Surface offset</param>
        /// <param name="score">Output score measure of how far the pose is to limits. Bigger is better</param>
        /// <returns>Tool control point pose</returns>
        /// <exception cref="ArgumentException">Hardware cannot execute the grip</exception>
        public override Pose ArmPoseFromGrip(GripState grip, double surfaceOffset, out double score)
        {
            Vector4 palmPlane = grip.PalmPlane;
            Vector3 gripCenterAxis = grip.GripCenterAxis;
            Vector3 gripCenter = grip.GripCenterGlobalLocation;

            Vector3 n = new Vector3(palmPlane.X, palmPlane.Y, palmPlane.Z);
            Vector3 tcpPosition = gripCenter - n * this.ArmTCPPalmOffsetInM;
            score = this.GetHeuristicMaximumReaches(tcpPosition, gripCenter, n);

            // Compute final rotation in 2 steps
            // Step 1 Rotation to align the palm normal
            Quaternion orient1 = Quaternion.QuaternionFromVectors(this.PalmPlaneDefault, n);

            Vector3 palmRotate = Quaternion.Rotate(orient1, this.PalmPlaneGripAxis);

            // Step 2 Rotation in palm plane to align the grip axis
            Quaternion orient2 = Quaternion.QuaternionFromVectors(palmRotate, gripCenterAxis);
            Quaternion orientation = orient2 * orient1;

            // This improves kuka arm cartesian point to point movement behavior
            if (orientation.W < 0.001)
            {
                orientation.W = 0.0;
            }

            // Approximate model to allow for gripper clearance from a surface parallel to X-Y plane
            tcpPosition.Z = Math.Max(this.ClearanceForSurfaceAvoidance(palmRotate, orientation, this.SimpleGripperBoxModel) + surfaceOffset, tcpPosition.Z);
            Pose armPose = new Pose(tcpPosition, orientation);

            return armPose;
        }

        /// <summary>
        /// Retrieve a pre grip plane from a grip plane
        /// </summary>
        /// <param name="gripPlane">Input grip plane</param>
        /// <returns>Pre grip plane</returns>
        public override Vector4 PreGripPlane(Vector4 gripPlane)
        {
            Vector4 preGripPalmPlane = this.OffsetPalmPlane(gripPlane, -1.0 * this.PreGripOffsetAlongGripNormalInM);
            return preGripPalmPlane;
        }

        /// <summary>
        /// Retrieve the pre grip center from a grip
        /// </summary>
        /// <param name="grip">The grip</param>
        /// <returns>Pre grip center</returns>
        public override Vector3 PreGripCenter(GripState grip)
        {
            Vector3 gripCenter = grip.GripCenterGlobalLocation;
            Vector4 gripPlane = grip.PalmPlane;
            Vector3 n = new Vector3(gripPlane.X, gripPlane.Y, gripPlane.Z);
            Vector3 preGripCenter = gripCenter - n * this.PreGripOffsetAlongGripNormalInM;
            return preGripCenter;
        }

        /// <summary>
        /// Return the amount that gripper should open expressed as a percent of the maximum open configuration in "open" state
        /// </summary>
        /// <param name="gripDimension">Dimension of the object at the grip center orthogonal to grip axis and parallel to the palm plane</param>
        /// <returns>Amount of grip opening expressed as a percent</returns>
        public override double GetPercentOpenForOpenState(double gripDimension)
        {
            return 100.0;
        }

        /// <summary>
        /// Return the amount that gripper should open expressed as a percent of the maximum open configuration in "close" state
        /// Assumes a simple linear model of gripper opening (in percent) as a function of opening in meter
        /// </summary>
        /// <param name="gripDimension">Requested gripper opining in meter</param>
        /// <returns>Amount of grip opening expressed as a percent</returns>
        public override double GetPercentOpenForCloseState(double gripDimension)
        {
            ////double close = (100.0 - this.GripperOpenPercentOffset) * gripDimension / this.GripperRatePerM + this.GripperOpenPercentOffset;
            double close = this.GripperRatePerM * gripDimension + this.GripperOpenPercentOffset;
            
            close = Math.Min(Math.Max(0, close), 100.0);
            return close;
        }

        /// <summary>
        /// Simplistic obstacle avoidance of XY surface at z = 0  Models the hand as a box
        /// oriented in same direction as the palm surface normal. Returns maximal distance from grip center
        /// </summary>
        /// <param name="planeNormal">Direction of palm plane normal</param>
        /// <param name="rotation">PLane rotation</param>
        /// <param name="gripperModel">Box gripper Model</param>
        /// <returns>Returns minimal z clearance height from model</returns>
        private double ClearanceForSurfaceAvoidance(Vector3 planeNormal, Quaternion rotation, Vector3 gripperModel)
        {
            Vector3 gripRotated = Quaternion.Rotate(rotation, gripperModel);

            return Math.Abs(gripRotated.Z);
        }

        /// <summary>
        /// Use heuristics to get  a quality score rating the position and orientation of the TCP
        /// The score is related to to the arm's margin to reach limits
        /// </summary>
        /// <param name="tcpPosition">Position of the tool control point</param>
        /// <param name="gripCenter">Grip center</param>
        /// <param name="palmNormal">Normal to palm</param>
        /// <exception cref="ArgumentException">Hardware cannot execute the grip</exception>
        /// <returns>Quality score for arm</returns>
        private double GetHeuristicMaximumReaches(Vector3 tcpPosition, Vector3 gripCenter, Vector3 palmNormal)
        {
            double armGripperOrient = Vector3.Dot(palmNormal, gripCenter);
            double verticalOrient = Math.Abs(Vector3.Dot(palmNormal, new Vector3(0, 0, 1)));
            double reachMax = tcpPosition.Length();
            double reachXY = Math.Sqrt(reachMax * reachMax - tcpPosition.Z * tcpPosition.Z);

            double minArmReachForOrient = this.MinArmReachInM;
            double maxArmReachForOrient = this.MaxArmReachInM;
            double maxArmReachInXY = this.MaxArmReachXYInM;
            double maxFullyExtendXYForTopDown = this.MaxFullyExtendXYForTopDown;
            bool isTopDown = false;

            if (Math.Abs(verticalOrient - 1.0) < 0.1)
            {
                isTopDown = true;
                minArmReachForOrient = this.MinArmReachInMForTopDown;
                maxArmReachForOrient = this.MaxArmReachInMForTopDown;
                maxArmReachInXY = this.MaxArmReachXYInMForTopDown;
            }

            if (reachXY <= minArmReachForOrient || reachXY >= maxArmReachInXY || reachMax >= maxArmReachForOrient ||
                (tcpPosition.Y > this.MaxYclearance && tcpPosition.Z < this.MinZClearanceOverTable) ||
                (isTopDown == false && armGripperOrient < this.MinArmGripperOrientThreshold) ||
                (isTopDown == true && reachXY > maxFullyExtendXYForTopDown && tcpPosition.Z > this.MaxFullyExtendElevationForTopDown) ||
                (isTopDown == false && reachXY < this.MinReachXYForWrap && tcpPosition.Z > this.MaxFullyRetractedElevationWrap &&
                tcpPosition.Y < 0))
            {
                throw new ArgumentException(
                    string.Format(
                    "Wrist position at {0:F3} {1:F3} {2:F3} cannot be reached by {3} arm",
                    tcpPosition.X,
                    tcpPosition.Y,
                    tcpPosition.Z,
                    this.ArmName));
            }

            double score = Math.Min((reachXY - minArmReachForOrient), (maxArmReachForOrient - reachMax));
            return score;
        }
    }
}
